#include "Park.h"
#include <Arduino.h>
#include "math.h"

float iParkToAlpha(float d,float q,float c){
    return cos(c) * d -sin(c) * q;
}
float iParkToBeta(float d,float q,float c){
    return sin(c) * d -cos(c) * q;
}